#include <iostream>
#include <eigen3/Eigen/Eigen>
#include <cmath>

struct state
{
    Eigen::Vector4d x_k;
    Eigen::Vector2d z_k;
    Eigen::Matrix4d P_k;
    Eigen::Matrix4Xd K_k = Eigen::Matrix4Xd::Zero(4,2);
    state* m_next = NULL;
};

class EKF
{
private:
    state *m_head;
    int count;
public:
    EKF();
    EKF(Eigen::Vector4d x0, Eigen::Matrix4d P0);
    ~EKF();
    void Removeall();
    void Getx_k(Eigen::Matrix4Xd &x);// 读出预测数值
    void Pushz_k(Eigen::Matrix2Xd &z);// 写入量测数值
    void EKFAlgo();
};